#include "door_behavior_tree.hpp"

#include <chrono>
#include <iostream>
#include <thread>

class OpenDoorAction : public BT::SyncActionNode {
 public:
  explicit OpenDoorAction(const std::string& name) : BT::SyncActionNode(name, {}) {
  }
  BT::NodeStatus tick() override {
    std::cout << "Action: Open the door." << std::endl;
    return BT::NodeStatus::SUCCESS;
  }
};

class CloseDoorAction : public BT::SyncActionNode {
 public:
  explicit CloseDoorAction(const std::string& name) : BT::SyncActionNode(name, {}) {
  }
  BT::NodeStatus tick() override {
    std::cout << "Action: Close the door." << std::endl;
    return BT::NodeStatus::SUCCESS;
  }
};

DoorBehaviorTree::DoorBehaviorTree() {
  BT::BehaviorTreeFactory factory;
  factory.registerNodeType<OpenDoorAction>("OpenDoorAction");
  factory.registerNodeType<CloseDoorAction>("CloseDoorAction");

  tree = factory.createTreeFromText(R"(
        <root BTCPP_format="4">
            <BehaviorTree ID="MainTree">
                <Sequence name="main_sequence">
                    <OpenDoorAction/>
                    <CloseDoorAction/>
                </Sequence>
            </BehaviorTree>
        </root>
    )");
}

void DoorBehaviorTree::run() {
  while (tree.rootNode()->status() == BT::NodeStatus::IDLE || tree.rootNode()->status() == BT::NodeStatus::RUNNING) {
    tree.rootNode()->executeTick();
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
  }
}
